#pragma once

#include <eigen3/Eigen/Dense>
#include <geometry_msgs/msg/point_stamped.hpp>
#include <geometry_msgs/msg/vector3_stamped.hpp>
#include <geometry_msgs/msg/quaternion_stamped.hpp>  // 新增：四元数消息
#include <hippo_common/convert.hpp>
#include <hippo_common/param_utils.hpp>
#include <hippo_common/tf2_utils.hpp>
#include <hippo_control_msgs/msg/attitude_target.hpp>
#include <hippo_control_msgs/msg/thrust_attitude_target.hpp>  // 新增：新的消息类型
#include <nav_msgs/msg/odometry.hpp>
#include <rcl_interfaces/msg/set_parameters_result.hpp>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/float64.hpp>


#include "hippo_control/trajectory_tracking_bluerov/tracking_control_bluerov.hpp"

namespace hippo_control {
namespace trajectory_tracking_bluerov {
class TrackingControllerBlueROVNode : public rclcpp::Node {  // 修改类名
 public:
  explicit TrackingControllerBlueROVNode(rclcpp::NodeOptions const &_options);  // 修改构造函数

 private:
 //参数在这里
  struct Params {
    bool updated{false};
    double position_gain{1.0};
    double velocity_gain{1.0};
  };
  void DeclareParams();
  void InitPublishers();
  void InitSubscriptions();
  void UpdateControllerParams();

  // 修改：更新发布函数以支持三维推力向量
  void PublishAttitudeTarget(const Eigen::Quaterniond &_attitude,
                             const Eigen::Vector3d &_thrust_world, 
                             const rclcpp::Time &_now);

  // 删除：OnRollTarget函数

  // 新增：目标姿态回调函数
  void OnAttitudeTargetDesired(
      const geometry_msgs::msg::QuaternionStamped::SharedPtr _msg);

  void OnOdometry(const nav_msgs::msg::Odometry::SharedPtr _msg);
  void OnPositionTarget(
      const geometry_msgs::msg::PointStamped::SharedPtr _msg) {
    hippo_common::convert::RosToEigen(_msg->point, position_target_);
  }

  rcl_interfaces::msg::SetParametersResult OnParams(
      const std::vector<rclcpp::Parameter> &_parameters);
  void OnVelocityTarget(
      const geometry_msgs::msg::Vector3Stamped::SharedPtr _msg) {
    hippo_common::convert::RosToEigen(_msg->vector, velocity_target_);
  }

  OnSetParametersCallbackHandle::SharedPtr params_cb_handle_{nullptr};

  // 修改：使用新的消息类型
  rclcpp::Publisher<hippo_control_msgs::msg::ThrustAttitudeTarget>::SharedPtr
      attitude_target_pub_{nullptr};
      
  // 删除：roll_target_sub_
  // 新增：目标姿态订阅
  rclcpp::Subscription<geometry_msgs::msg::QuaternionStamped>::SharedPtr 
      attitude_target_desired_sub_{nullptr};

  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odometry_sub_{
      nullptr};
  rclcpp::Subscription<geometry_msgs::msg::Vector3Stamped>::SharedPtr
      velocity_target_sub_{nullptr};
  rclcpp::Subscription<geometry_msgs::msg::PointStamped>::SharedPtr
      position_target_sub_{nullptr};
  Params params_;
  TrackingControllerBlueROV controller_;  // 修改控制器类名

  Eigen::Vector3d position_{0.0, 0.0, 0.0};
  Eigen::Vector3d velocity_{0.0, 0.0, 0.0};
  Eigen::Quaterniond attitude_{1.0, 0.0, 0.0, 0.0};
  Eigen::Vector3d position_target_{0.0, 0.0, 0.0};
  Eigen::Vector3d velocity_target_{0.0, 0.0, 0.0};
  Eigen::Quaterniond attitude_target_{1.0, 0.0, 0.0, 0.0};
  // 新增：期望姿态
  Eigen::Quaterniond attitude_desired_{1.0, 0.0, 0.0, 0.0};
  bool received_attitude_target_{false};

};
}  // namespace trajectory_tracking
}  // namespace hippo_control
